package rollercoaster.railway;

import rollercoaster.commons.Vertex3D;
import rollercoaster.commons.interfaces.ICurve3D;
import rollercoaster.commons.interfaces.ITrajectory;
import rollercoaster.railway.utils.VertexUtils;

public class Trajectory implements ITrajectory{
	
	ICurve3D base, up;
	
	public Trajectory(ICurve3D base, ICurve3D up){
		this.base = base;
		this.up = up;
	}

	public void getTransformMatrix(float[][] matrix, float t)
			throws IndexOutOfBoundsException {
		
		for (int i = 0; i < matrix.length; i++) {
			if(matrix.length != 4 || matrix[i].length != 4){
				throw new IndexOutOfBoundsException("4");
				
			}
		}
		//Error checking...
		Vertex3D z = base.dGammadt(t);
		VertexUtils.normalizeVector(z);
		Vertex3D y = new Vertex3D(up.gamma(t).getX() - base.gamma(t).getX(), up.gamma(t).getY() - base.gamma(t).getY(), up.gamma(t).getZ() - base.gamma(t).getZ());
		VertexUtils.normalizeVector(y);
		Vertex3D x = VertexUtils.crossProduct(y, z);
		VertexUtils.normalizeVector(x);
		y = VertexUtils.crossProduct(z, x);
		
		matrix[0][0]=x.getX();
		matrix[0][1]=y.getX();
		matrix[0][2]=z.getX();
		matrix[0][3]=base.gamma(t).getX();
		matrix[1][0]=x.getY();
		matrix[1][1]=y.getY();
		matrix[1][2]=z.getY();
		matrix[1][3]=base.gamma(t).getY();
		matrix[2][0]=x.getZ();
		matrix[2][1]=y.getZ();
		matrix[2][2]=z.getZ();
		matrix[2][3]=base.gamma(t).getZ();
		matrix[3][0]=0;
		matrix[3][1]=0;
		matrix[3][2]=0;
		matrix[3][3]=1;
	
		
	}

	public ICurve3D getBase() {
		return base;
	}

	public ICurve3D getUp() {
		return up;
	}
	
	
}
